import socket
import threading
import struct
import cv2
import time
import numpy as np

HOST = "169.254.46.218"
CAMERA_PORT_LEFT = 10000
CAMERA_PORT_RIGHT = 10001
HEAD_PORT = 11000

left_img_bytes = None
right_img_bytes = None
times = 0

# cv获取视频
def camera():
    global left_img_bytes
    global right_img_bytes
    cap = cv2.VideoCapture(0)
    # 设置摄像头分辨率
    cap.set(cv2.CAP_PROP_FRAME_WIDTH, 3040)
    cap.set(cv2.CAP_PROP_FRAME_HEIGHT, 1520)
    while True:
        ret, frame = cap.read()

        if ret:
            left_img_origin = frame[:, 0:1520, :]
            left_img_lite = cv2.resize(left_img_origin,(380,380))
            right_img_origin = frame[:, 1520:3040, :]
            right_img_lite = cv2.resize(right_img_origin,(380,380))
            left_img_bytes = cv2.imencode('.jpg', left_img_lite)[1].tobytes()
            right_img_bytes = cv2.imencode('.jpg', right_img_lite)[1].tobytes()
            

# 发送图像
def sendImg(s,isLeft,port):
    global left_img_bytes
    global right_img_bytes
    global times

    while True:
        print(1)
        if isLeft == True:
            if left_img_bytes != None:
                data = s.recv(1024)
                llen = len(left_img_bytes)  
                msg = 'l'.encode('utf-8')    
                print("left size is %d"%(llen))      
                s.send(msg + llen.to_bytes(4, byteorder='little',signed=False) + left_img_bytes)
                #s.send(msg + llen.to_bytes(4, byteorder='little',signed=False))
                a = llen.to_bytes(4, byteorder='little',signed=False)
                print(msg)
                for byte in a:
                    print("%x"%(byte))
                
        else:
            if right_img_bytes != None:
                data = s.recv(1024)
                rlen = len(right_img_bytes)
                print("right size is %d"%(rlen))
                msg = 'r'.encode('utf-8')
                s.send(msg + rlen.to_bytes(4, byteorder='little',signed=False) + right_img_bytes)
                
        print("send one frame %d, for the %d th time" %(isLeft,times))
        #time.sleep(0.05)
        times = times + 1
        

# 接收头盔旋转和位置信息
def getHead(s):

    while True:
        data = s.recv(1024)
        pos_x = struct.unpack('f', data[0][0:4])[0]
        pos_y = struct.unpack('f', data[0][4:8])[0]
        pos_z = struct.unpack('f', data[0][8:12])[0]
        rot_x = struct.unpack('f', data[0][12:16])[0]
        rot_y = struct.unpack('f', data[0][16:20])[0]
        rot_z = struct.unpack('f', data[0][20:24])[0]
        print(str(pos_x)+'\t'+str(pos_y)+'\t'+str(pos_z)+'\t'+str(rot_x)+'\t'+str(rot_y)+'\t'+str(rot_z))

def listenClient(port, func, args):
    s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
    s.bind((HOST, port))
    s.listen(True)
    
    while True:
        conn, addr = s.accept()
        threading.Thread(target=func, args=((conn,)+args)).start()

if __name__ == "__main__":
    threads = []

    threads.append(threading.Thread(target=listenClient, args=(CAMERA_PORT_LEFT,sendImg,(True,CAMERA_PORT_LEFT,),)))
    threads.append(threading.Thread(target=listenClient, args=(CAMERA_PORT_RIGHT,sendImg,(False,CAMERA_PORT_RIGHT,),)))
    threads.append(threading.Thread(target=listenClient, args=(HEAD_PORT,getHead,(),)))
    threads.append(threading.Thread(target=camera))
    #threads.append(threading.Thread(target=sendImg,args = (True,CAMERA_PORT_LEFT,)))
    #threads.append(threading.Thread(target=sendImg,args = (False,CAMERA_PORT_RIGHT,)))
    #threads.append(threading.Thread(target=getHead))
    #threads.append(threading.Thread(target=getControl, args=(12000,)))
    #threads.append(threading.Thread(target=getControl, args=(13000,)))

    for t in threads:
        t.start()

    for t in threads:
        t.join()
